/*
 * Lithne.cpp - The official Lithne library.
 *
 * Copyright (c) 2011
 * Serge Offermans, Remco Magielse
 * {s.a.m.offermans, r.magielse}@tue.nl
 * Intelligent Lighting Institute (ILI), TU/e.
 *
 * All rights reserved. LAST UPDATE: 30-03-2012
 *
 *	TO DO:
 *	- Make integrated 'send' function, where all data is passed in the arguments
 *	- 16B DBrequest function
 *	-	Clean up setting the recipient
 */

/* Include other libraries */
#include "Lithne.h"
#include "Message.h"
#include "Node.h"
#include "XBee.h"
#include <stdlib.h>
#include "LithneDefinitions.h"

//#define DEBUG_HUGS
//#include "../../HUGsCode/trunk/libraries/HUGSlibs/HUGS_debugutils.h"

#if defined(ARDUINO) && ARDUINO >= 100
#define ARDUINO1DOT0 true
#else
#include "cppfix.h"
#define ARDUINO1DOT0 false
#endif

//#define UNKNOWN_NODE_ID 254	 //The standard ID for an unknown node

/* uint8_t */
uint8_t DIGITAL[6]	=	{2, 4, 7, 8, 12, 13};	//defines the digital pins
uint8_t PWM[6]		=	{3, 5, 6, 9, 10, 11};	//defines the pwm pins
uint8_t atSH[]      =	{'S','H'};
uint8_t atSL[]      =	{'S','L'};
uint8_t atMY[]      =	{'M','Y'};
uint8_t atID[]      = 	{'I','D'};
uint8_t atAI[]      = 	{'A','I'};
uint8_t atDB[]		=	{'D','B'};

/*	uint16_t	*/

/* Define objects */
XBee xbee 						= XBee();
//ZBRxResponse rx 				= ZBRxResponse();  ----> moved to ::readXBee()
//ZBTxStatusResponse txStatus 	= ZBTxStatusResponse();
ModemStatusResponse msr 		= ModemStatusResponse();
RemoteAtCommandResponse rATcmd	= RemoteAtCommandResponse();

/* 
  ___ ___  _  _ ___ _____ ___ _   _  ___ _____ ___  ___  ___ 
 / __/ _ \| \| / __|_   _| _ \ | | |/ __|_   _/ _ \| _ \/ __|
| (_| (_) | .` \__ \ | | |   / |_| | (__  | || (_) |   /\__ \
 \___\___/|_|\_|___/ |_| |_|_\\___/ \___| |_| \___/|_|_\|___/

 */
/** 
	Default constructor called when importing the library. Used to initialize the nodes and scopes arrays and create incoming and outgoing message objects
 **/
LithneClass::LithneClass(XBEE_SERIES series):mSeries(series)
{
	for( uint8_t i; i<MAX_NODES; i++ )
	{
		nodes[i]	=	0;		//Node();
	}
	for( uint8_t i; i<MAX_SCOPES; i++ )
	{
		scopes[i]	=	0;
	}
	incomingMessage = 	new Message();
	outgoingMessage = 	new Message();
	numNodes 		= 	0;
	newMessage 		= 	false;
	debugMode		=	false;

	myAddress64		=	XBeeAddress64( 0, UNKNOWN_64B );
	myAddress16		=	UNKNOWN_16B;
	myPANid			=	UNKNOWN_PAN_ID;
	myAssStat		=	UNKNOWN_STATUS;
}

LithneClass::~LithneClass()
{
	delete incomingMessage;
	delete outgoingMessage;
	/*	There is little to no need to clean up here, 
		as we don't expect to ever dispose of this class.	*/
}

/* 
 ___ _   _ ___ _    ___ ___   ___ _   _ _  _  ___ _____ ___ ___  _  _ ___ 
| _ \ | | | _ ) |  |_ _/ __| | __| | | | \| |/ __|_   _|_ _/ _ \| \| / __|
|  _/ |_| | _ \ |__ | | (__  | _|| |_| | .` | (__  | |  | | (_) | .` \__ \
|_|  \___/|___/____|___\___| |_|  \___/|_|\_|\___| |_| |___\___/|_|\_|___/

 */

/** Specify the baud rate at which to communicate to the XBee **/
void LithneClass::init( uint32_t _baud, HardwareSerial &_port )
{
	xbee.setSerial( _port );
	xbee.begin( _baud );
}

/** Specify the baud rate at which to communicate to the XBee **/
void LithneClass::begin( uint32_t _baud, HardwareSerial &_port )
{
	xbee.setSerial( _port );
	xbee.begin( _baud );
}

/** Set the function to call in the outgoing message **/
void LithneClass::setFunction( uint8_t _function )
{
	outgoingMessage->setFunction( _function );
}

/** Set the address of the receiving XBee by using the identifier (not the same as array position in the Nodes array!) **/
void LithneClass::setRecipient( uint8_t _id )
{
	/*	Retrieve the node the user refers to	*/
	Node * receivingNode	=	getNodeByID( _id );
	/*	From this node, get the 16-bit and 64-bit address and
		write it to the outgoing message	*/
	outgoingMessage->setRecipient64( receivingNode->getAddress64() );
	outgoingMessage->setRecipient16( receivingNode->getAddress16() );
}
/** Set the address of the receiving XBee by using the full 64 bit address **/
void LithneClass::setRecipient( XBeeAddress64 _add64 )
{
	/*	From this node, get the 16-bit and 64-bit address and
		write it to the outgoing message	*/
	outgoingMessage->setRecipient64( _add64 );
	outgoingMessage->setRecipient16( getNodeBy64( _add64 )->getAddress16() );
}
/** Deprecated private function to set the recipient using a pointer to a node **/
void LithneClass::setRecipient( Node * _receivingNode )
{
	outgoingMessage->setRecipient64 ( _receivingNode->getAddress64() );
	outgoingMessage->setRecipient16 ( _receivingNode->getAddress16() );
}

/**	Retrieves the 64-bit address based on the 16-bit address **/
void LithneClass::setRecipient16( uint16_t _add16 )
{	
	//	XBeeAddress64 recipient = getNodeAddress64( _add16 );
	outgoingMessage->setRecipient64( getNodeBy16( _add16 )->getAddress64() );
	outgoingMessage->setRecipient16( _add16 );
}

/** Add an argument to the message **/
void LithneClass::addArgument( uint16_t _arg )
{
	outgoingMessage->addArgument( _arg );
}

/** Transmit the outgoing message, use setRecipient and setFunction before calling this function  **/
void LithneClass::send( )
{
	sendMessage( outgoingMessage );
	/* After transmitting the message, we immediately clear the arguments */
	outgoingMessage->clearArguments();
	/*	After transmitting the message we set the scope to NO_SCOPE, so 
		our next message is always received by any other node (unless we specify
		a scope again)	*/
	outgoingMessage->setScope( NO_SCOPE );
}
/**	Transmit outgoing message with the specified recipient by 64 bit address and function **/
void LithneClass::send( XBeeAddress64 recipient, uint8_t function )
{
	setRecipient( recipient );
	setFunction( function );
	send();
}
/**	Transmit outgoing message with the specified recipient ID and function **/
void LithneClass::send( uint8_t _id, uint8_t _function )
{
	setRecipient( _id );
	setFunction( _function );
	send();
}

/** EXPERT FUNCTION - DON'T CALL UNLESS YOU KNOW WHAT YOU ARE DOING!
	Transmits the message in the argument and consequently read the
	response from the XBee.	
	Message * message: Message to be transmitted.
 **/
void LithneClass::sendMessage( Message * message )
{
	// break down 10-bit reading into two bytes and place in payload
	uint16_t payloadSize = (message->getNumberOfArguments()*2) + 3;
	uint8_t  payload[payloadSize];

	/*	The first bytes of the payload are fixed for the message scope
		and the function call. Here we write those.	*/
	payload[0]	=	( message->getScope() >> 8 ) & 0xFF;
	payload[1]	=	message->getScope() & 0xFF;
	payload[2]  = 	message->getFunction();

	/*	Then, we write the arguments from the message to the payload	*/
	for( int i=0; i < message->getNumberOfArguments(); i++ )
	{
		uint16_t arg		=	message->getArgument(i);
		payload[(i*2)+3]	= 	( arg >> 8 ) & 0xFF;
		payload[(i*2)+4]   	= 	arg & 0xFF;
	}

	/* Retrieve the 64-bit address from the message*/
	XBeeAddress64 addr64 	=	message->getRecipient64();
	uint16_t	  addr16	=	message->getRecipient16();

	/*	We always supply the 64-bit and 16-bit address. In case the
		16-bit address is NOT known, the ZBTxRequest will automatically
		use the 64-bit address.
	 */


	if (mSeries == S2)
	{
		ZBTxRequest Tx = ZBTxRequest( addr64, addr16, 0, 0, payload, sizeof(payload), 0x01 );

		xbee.send(Tx);
	}
	else if (mSeries == S1)
	{
		if (addr16 != UNKNOWN_16B)	//16-bit address is known, let's use that to send the message
		{
			Tx16Request Tx = Tx16Request(addr16, 0, payload, sizeof(payload), 0x01);

			xbee.send(Tx);
		}
		else if(addr64.getLsb()	!=	UNKNOWN_64B )
		{
			Tx64Request Tx = Tx64Request(addr64, 0, payload, sizeof(payload), 0x01);

			xbee.send(Tx);
		}
	}

	/* Store the addresses we transmit to */	
	last16B	=	addr16;
	last64B	=	addr64;

	/* After transmitting, read any possible status response */
	readXbee();
}

/** Transmit a DB request (RSSI, signal strength) by using the node identifier **/
void LithneClass::sendDBRequest( uint8_t _id )
{
	sendDBRequest( getNodeByID( _id ) );
	/*	This code has been changed, but not tested!
 	uint16_t _addr16 		=	receivingNode->getAddress16();

 	if ( _addr16 != UNKNOWN_NODE ) 
	{
		sendDBRequest16( _addr16 );
	}
	// the 16-bit address is not known, so we send a 64B request 
	else 
 	{
 		sendDBRequest( receivingNode->getAddress64() );
 	}
	 */
}
/** Transmit a DB request (RSSI, signal strength) by using the 64-bit address **/
void LithneClass::sendDBRequest( XBeeAddress64 _addr64 )
{
	sendDBRequest( getNodeBy64( _addr64 ) );
	/* !!!This code has been changed, but not tested	*
	Node * receivingNode	=	getNodeBy64( _addr64 );
	uint16_t _addr16 		=	receivingNode->getAddress16();

 	if ( _addr16 != UNKNOWN_NODE ) 
	{
		sendDBRequest16( _addr16 );
	}
	// The 16B address is not known, so we send a 64B request 
	else 
	{
		RemoteAtCommandRequest db = RemoteAtCommandRequest( addr64, atID );
    	// Send your request
		xbee.send(db);

		uint8_t remoteNode = getNodeId( addr64 );

		/*	If we broadcast the DB request, open the node request on
			all nodes	*
    	if ( _addr64.getMsb() == 0 && _addr64.getLsb() == 0xFFFF )
    	{
			for (int i =0; i< numNodes; i++)
			{
				nodes[i]->openDBRequest();
    		}
    	}
    	/*	Otherwise, just open the request on the node we
    		transmit the DB request to	*
    	else
    	{
			for (int i =0; i< numNodes; i++)
	 		{
				if (remoteNode == nodes[i]->getID())
				{
			  		nodes[i]->openDBRequest();
				}
    		}
    	}
    }
	 */
}
/** Transmit a DB request (RSSI, signal strength) by using the 16-bit address **/
void LithneClass::sendDBRequest16( uint16_t _addr16 )
{
	sendDBRequest( getNodeBy16( _addr16) );
	/*	!!! Code has been changed, but not tested!
	RemoteAtCommandRequest db = RemoteAtCommandRequest( _addr16, atDB );
    // Send your request
    xbee.send(db);

	if( nodeKnown( _addr16 ) )
	{
    	uint8_t remoteNode = getNodeId( _addr16 );

		for (int i =0; i< numNodes; i++)
	 	{
			if (remoteNode == nodes[i]->getID())
			{
		  		nodes[i]->openDBRequest();
			}
   		}
	}
	 */
}
/*	Send a DB request to a specific node. This is the new function all the
	other request forward their code to. **/
void LithneClass::sendDBRequest( Node * _receivingNode )
{
	XBeeAddress64	addr64	=	_receivingNode->getAddress64();
	uint16_t		addr16	=	_receivingNode->getAddress16();


	RemoteAtCommandRequest db;

	/*	We prefer a 16-bit address, so check if we have that...	*/
	if( addr16 != UNKNOWN_NODE )
	{
		db = RemoteAtCommandRequest( addr16, atDB );
	}
	else  /* ... else we send to the 64-bit address */
	{
		db = RemoteAtCommandRequest( addr64, atDB );
	}

	/*	If we broadcast the DB request, open the node request on all nodes	*/
	if ( addr64.getMsb() == 0 &&
			addr64.getLsb() == 0xFFFF )
	{
		for (int i =0; i< numNodes; i++)
		{
			nodes[i]->openDBRequest();
		}
	}
	/*	Otherwise, just open the request on the node we	transmit the DB request to	*/
	else
	{
		_receivingNode->openDBRequest();
	}

	/*	Transmit the request	*/
	xbee.send( db );
}

/**	Collect information of the XBee connected to the local node.
	However, this information is only written when the XBee is read.**/
void LithneClass::getMyInfo()
{
	sendATCommand( atSH );
	sendATCommand( atSL );
	sendATCommand( atMY );
	sendATCommand( atID);
	sendATCommand( atAI );
}

/**	Sets the hash code of the group (scope) to the outgoing message	**/
void LithneClass::setScope( uint16_t _scope )
{
	outgoingMessage->setScope( _scope );
}

/**	Add the	new scope to the existing array	**/
void LithneClass::addScope( uint16_t _scope )
{
	/*	Go through all the scopes, until we find an empty location, 
		write the scope there	*/
	for( int i=0; i<MAX_SCOPES; i++ )
	{
		if( scopes[i]	==	0)
		{
			scopes[i]	=	_scope;
			break;
		}
	}
}

/**	Creates a hash code of the specified string and adds this to the internal hash register.
	arg: String with maximum 20 characters	**/
void LithneClass::joinGroup( String _group )
{
	addScope( hashGroup(_group) );
}

/**	Broadcasts a hello world command and either requests a reply.
	If a reply is requested, a node will reply with hello world,
	but should NOT ask for a reply (otherwise you end up in an infinite
	loop).
 **/
void LithneClass::helloWorld( XBeeAddress64 _addr64, bool askReply )
{
	setRecipient( _addr64 );
	setFunction ( HELLO_WORLD );
	addArgument(  getMyAddress16() );
	addArgument(  askReply );
	send();
}





/*
 ___  ___   ___  _    ___   _   _  _ 
| _ )/ _ \ / _ \| |  | __| /_\ | \| |
| _ \ (_) | (_) | |__| _| / _ \| .` |
|___/\___/ \___/|____|___/_/ \_\_|\_|

 */

/** Check whether new data is available. If so, raise newMessage flag **/
bool LithneClass::available()
{

	readXbee();

	if( newMessage )
	{
		newMessage = false; // reset flag
		return true;
	}
	else
	{
		return false;
	}

}

/** Add a new node to the memory, giving it a specific ID 
	If succesful, return true, otherwise return false.	**/
bool LithneClass::addNode( uint8_t _id, XBeeAddress64 _addr64, uint16_t _addr16 )
{
	/*	If we have not passed the maximum number of nodes	*/
	if( numNodes < MAX_NODES && 
			!nodeKnown( _id ) &&
			!nodeKnown64( _addr64)	)
	{
		nodes[numNodes] = new Node( _id, _addr64, _addr16 );
		numNodes++;
		return true;	//Return true, because we have succesfully added a node
	}
	else
	{
		return false;	//If we can't add a new node, return false
	}
}

/**	Checks whether there is a node with the specified ID in the memory	**/
bool LithneClass::nodeKnown( uint8_t _id )
{
	/*	getNodeByID returns the node with the specific identifier, or a new
		node with unknown data (id and address). If this is the case, the
		node is thus not known. Otherwise, it is known	*/
	if( getNodeByID( _id )->getID() == UNKNOWN_NODE_ID )
	{
		return false;
	}
	else
	{
		return true;
	}
}
/** Checks whether the node with the specified 64-bit address exists in the nodes[]. **/
bool LithneClass::nodeKnown64( XBeeAddress64 _addr64 )
{
	if( getNodeBy64( _addr64 )->getID() == UNKNOWN_NODE_ID )
	{
		return false;
	}
	else
	{
		return true;
	}

	/*	!!! CODE REPLACED, UNTESTED
	for( int i=0; i<numNodes; i++)
	{
	/** For each of the know nodes, check if the MSB and LSB
		are similar to the specified address in add64. **=
		if( _addr64.getMsb() == nodes[i]->getMSB() 
		&&  _addr64.getLsb() == nodes[i]->getLSB() )
		{
			return true;
			break;
		}
	}

	return false;
	 */
}
/** Checks whether the node with the specified 16-bit address exists in the nodes[]. **/
bool LithneClass::nodeKnown16( uint16_t _addr16 )
{
	if( getNodeBy16( _addr16 )->getID() == UNKNOWN_NODE_ID )
	{
		return false;
	}
	else
	{
		return true;
	}

	/*	!!! CODE REPLACED, UNTESTED
	bool known	=	false;

	for( int i=0; i<numNodes; i++)
	{
		Node * curNode	=	getNode( i );

		if( curNode->getAddress16() == _addr16 )
		{
		/** For each of the know nodes, check if the 16-bit address is 
			similar to the specified address in _addr16. **
			known	=	true;
			break;
		}
	}

	return known;
	 */
}

/** Check if a new DB measurement is available for a particular node (identified by Node ID) **/
bool LithneClass::newDBMeasurement( uint8_t _id )
{
	bool newMeasure = false;	//reset the flag

	for (int i = 0; i < numNodes; i++)
	{
		if ( getNode(i)->getID() == _id )
		{
			if( getNode( i )->isNewMeasurement() )
			{
				getNode( i )->closeDBRequest();
				newMeasure = true;
			}
		}
	}
	return newMeasure;
}
/**	Checks whether the provided hash scope is included
	in the known scopes.	**/
bool LithneClass::hasScope( uint16_t _scope )
{
	for(int i = 0; i < MAX_SCOPES; i++)
	{
		if (scopes[i]	== _scope)
		{
			return true;
		}
		/*		this is not neccesarily valid, because we might also
		remove scopes in the center of the array and then this
		function will never reach the later scopes
		else if (scopes[i] == 0)
		{
			break;
		}
		 */
	}
	return false;
}

/**	Removes the group from a specific scope	**/
bool LithneClass::removeScope( uint16_t _scope )
{
	for(int i = 0; i < MAX_SCOPES; i++)
	{
		if( scopes[i]	==	_scope )
		{
			scopes[i]	=	0;
			return true;
		}
	}
	return false;
}
/**	Removes the group from a specific scope	**/
bool LithneClass::removeScope( String _group )
{
	return removeScope( hashGroup(_group) );
}




/*
 ___ _  _ _____ ___ ___ ___ ___ 
|_ _| \| |_   _| __/ __| __| _ \
 | || .` | | | | _| (_ | _||   /
|___|_|\_| |_| |___\___|___|_|_\

 */

/** Returns the pin number of the digital out pins. 
	This makes it easy to refer	to the digital output 
	pins as 0-6, as	they are mapped on the Lithne node **/
uint8_t	LithneClass::digitalPin( uint8_t position )
{
	position = constrain( position, 0, 5 );
	return DIGITAL[position];
}

/** Returns the pin number of the pwm (DAC) out pins.
	This makes it easy to refer	to the digital output 
	pins as 0-6, as	they are mapped on the Lithne node **/
uint8_t	LithneClass::pwmPin( uint8_t position )
{
	position = constrain( position, 0, 5 );
	return PWM[position];
}

/** Returns the number of arguments in the incoming message **/
uint8_t LithneClass::getNumberOfArguments()
{
	return incomingMessage->getNumberOfArguments();
}

/** Returns the number of nodes currently known **/
uint8_t LithneClass::getNumberOfNodes()
{
	return numNodes;
}

/* DEPRECATED. Use getNodeBy16( _addr16 )->getID() instead.
Return the ID number of the node, based on the 16-bit address **
uint8_t LithneClass::getNodeId( uint16_t _addr16 )
{
	return getNodeBy16( _addr16 )->getID();
	/* !!! CODE REVISED, UNTESTED
	uint16_t id = UNKNOWN_NODE_ID;
	uint8_t pos	=	0;

	for (int i = 0; i < numNodes; i++)
	{
		if( _addr16 == nodes[i]->getAddress16() )
      	{
			id = nodes[i]->getID();
			pos	=	i;
		}
	}
	return id;
}
 */

/* DEPRECATED. Use getNodeBy64( _addr64 )->getID() instead
Return the ID number of the node, based on the 64-bit address **
uint8_t LithneClass::getNodeId( XBeeAddress64 _addr64 )
{
	return getNodeBy64( _addr64 )->getID();
	/*	!!! CODE REVISED, UNTESTED
	uint16_t id = UNKNOWN_NODE_ID;
	uint8_t	pos	=	0;

	for (int i = 0; i < numNodes; i++)
	{
		if( addr64.getMsb() == nodes[i]->getAddress64().getMsb() 
		 && addr64.getLsb() == nodes[i]->getAddress64().getLsb() )
      	{
			id = nodes[i]->getID();
			pos	=	i;
		}
	}

	return id;
}
 */

/*	FUNCTION HAS BECOME OBSOLETE...NO LONGER IN USE (DEPRECATED)
	This function looks up the specified 64-bit address and if neccesary 
	adds the 16-bit address to this node.
	RETURNS: the position of the node in the array

uint8_t LithneClass::setNodeAddress( XBeeAddress64 _add64, uint16_t _add16 )
{
	uint8_t nodePositionInArray	=	UNKNOWN_NODE;

	/*	If the address least significant byte is 0, this is a broadcast message.
		Then we shouldn't overwrite the 16-bit address *

		/* TO DO: Compare the _add64 with a broadcast address instead of number *
	if( _add64.getMsb() != 0 )
	{
		for( uint8_t i=0; i<numNodes; i++)
		{
			if( _add64.getMsb() == nodes[i]->getAddress64().getMsb() 
			 && _add64.getLsb() == nodes[i]->getAddress64().getLsb() )
			 {
			 	nodePositionInArray	=	i;
			 	nodes[i]->setAddress16( _add16 );
			 	break;
			 }
		}
	}

	return nodePositionInArray;
}
 */

/** Return the function of the incoming message **/
uint16_t LithneClass::getFunction()
{
	return incomingMessage->getFunction();
}

/**	Return the scope of the incoming message	**/
uint16_t LithneClass::getScope()
{
	return incomingMessage->getScope();
}

/** Return the argument at the specified position **/
uint16_t LithneClass::getArgument( uint8_t arg )
{
//	Serial.print("LithneClass::getArgument. Argument = ");
//	Serial.
	return incomingMessage->getArgument(arg);
}

/* 	DEPRECATED. Retrieve the address by using getNode()
Returns the 16-bit address based on the 64-bit address 
 *
uint16_t LithneClass::getNodeAddress16( XBeeAddress64 _add64 )
{
	uint16_t _add16 = UNKNOWN_NODE;

	for(uint8_t i = 0; i < numNodes; i++)
	{
		if ( _add64.getMsb() == nodes[i]->getAddress64().getMsb() 
		&& 	 _add64.getLsb() == nodes[i]->getAddress64().getLsb() )
      	{
			_add16 = nodes[i]->getAddress16();
			break;
		}
	}

	return _add16;
}
 */

/* 	DEPRECATED. Retrieve the address by using getNode()
Returns the 16-bit address based on the nodeId  *
uint16_t LithneClass::getNodeAddress16( uint8_t _nodeId )
{
	uint16_t add16 = UNKNOWN_NODE;

	for (int i = 0; i < numNodes; i++)
	{
		if ( _nodeId	== nodes[i]->getID() )
      	{
			add16 = nodes[i]->getAddress16();
		}
	}

	return add16;
}
 */

/* DEPRECATED. Use getNodeByID(id)->getDB() instead.
Return the DB measure for the specified node **
uint16_t LithneClass::getDB( uint8_t id )
{
	for( int i = 0; i < numNodes; i++ )
	{
		if (id == nodes[i]->getID())
		{
			return nodes[i]->getDB();
		}
	}
}
 */

/**	Returns the 16-bit address of the own node	**/
uint16_t LithneClass::getMyAddress16( bool forceCheck )
{
	if( myAddress16	==	UNKNOWN_16B || forceCheck )
	{	//We request the 16-bit address and wait 1 second for a response
		myAddress16	=	sendATCommand( atMY, 1000 ) & 0xFFFF;
	}

	return myAddress16;
}

/**	Returns the PAN ID of the xbee network	**/
uint16_t LithneClass::getMyPAN( bool forceCheck )
{
	if( myPANid	==	UNKNOWN_PAN_ID || forceCheck )
	{	//We request the PAN ID and wait 1 second for a respons
		myPANid	=	sendATCommand( atID, 1000 ) & 0xFFFF;
	}

	return myPANid;
}

/**	Returns the association status of the XBee	**/
uint16_t LithneClass::getMyAssociationStatus( bool forceCheck )
{
	if( myAssStat == UNKNOWN_STATUS || forceCheck )
	{	//We request the association status and wait 1 second for a response
		myAssStat	=	sendATCommand( atAI, 1000 ) & 0xFFFF;
	}
	return myAssStat;
}

/**	Returns a hash code based on the name of a group	**/
uint16_t LithneClass::hashGroup( String _group )
{
	uint16_t wordValue  =  0;
	for( int i=0; i < _group.length(); i++ )
	{
		wordValue  +=  _group.charAt(i);
	}
	wordValue += _group.length();
	wordValue += _group.charAt(0);
	wordValue += _group.charAt( _group.length() - 1 );

	return wordValue;
}

/**	Returns the 16-bit address of the sender as stored in the message	**/
uint16_t LithneClass::getSender16()
{
	return incomingMessage->getSender16();
}

/**	Sends an ATCommand and returns the response as a 32-bit integer.
	If you specify a value of waitForResponse, the program will wait
	for the amount of ms specified by waitForResponse and immediately
	process the AT response. If you don't specify a value or specify 0
	the program will call the general readXbee() and process the response
	there and write it to the corresponding variables.
	This function will then return 0.	**/
uint32_t LithneClass::sendATCommand( uint8_t * cmd, uint16_t waitForResponse )
{
	uint32_t	atAnswer	=	0;

	/*	Create a new AtComand Request and Response	*/
	AtCommandRequest atRequest   	= AtCommandRequest();

	/*	Set the specific command you want to send to the XBee in the atRequest	*/
	atRequest.setCommand( cmd );

	/*	Send this request to the xbee	*/
	xbee.send( atRequest );

	/*	The processing of the AT response is handled in the readXBee().
		We thus call this function. If we manually tell this function to
		wait for a response, it will skip this and attempt to read the
		information at this point. */
	if( waitForResponse == 0 )
	{
		readXbee();
	}
	else
	{
		if ( xbee.readPacket(waitForResponse) )
		{
			AtCommandResponse atResponse 	= AtCommandResponse();

			/*	If the packet is indeed an AT_COMMAND_RESPONSE,	we write it to the atResponse.	*/
			if (xbee.getResponse().getApiId() == AT_COMMAND_RESPONSE) 
			{
				xbee.getResponse().getAtCommandResponse(atResponse);
				/*	If the atResponse is correct (does not return an error code), we can process it.	*/
				if (atResponse.isOk()) 
				{
					if (atResponse.getValueLength() > 0) 
					{  
						for (int i = 0; i < atResponse.getValueLength(); i++) 
						{
							atAnswer	=	(atAnswer << 8) + atResponse.getValue()[i];
						}
					}
				}
			}
		}
		else 
		{
			// at command failed
			if (xbee.getResponse().isError())
			{
				//      	Serial.print("Error reading packet.  Error code: ");
				//  	    Serial.println(xbee.getResponse().getErrorCode());
			}
			else
			{
				//      	Serial.print("No response from radio");
			}
		}
	}

	return atAnswer;

}

/* 
__  _____            _      _    _                 __ _ _  
\ \/ / _ ) ___ ___  /_\  __| |__| |_ _ ___ ______ / /| | | 
 >  <| _ \/ -_) -_)/ _ \/ _` / _` | '_/ -_|_-<_-</ _ \_  _|
/_/\_\___/\___\___/_/ \_\__,_\__,_|_| \___/__/__/\___/ |_| 

 */

/** Return the address of the sender of the message **/
XBeeAddress64 LithneClass::getSender64()
{
	return incomingMessage->getSender64();
}

/* DEPRECATED. Use getNodeByID(id)->getAddress64() instead.
Return the node address of the specified ID **
XBeeAddress64 LithneClass::getNodeAddress64( uint8_t id )
{	//Create a new XBeeAddress64
	XBeeAddress64 recip = XBeeAddress64( 0, 0 );

	for (int i = 0; i < numNodes; i++)
	{
		if ( id == nodes[i]->getID() )
		{
			recip = nodes[i]->getAddress64();
		}
		else
		{

		}
	}
	return recip;
}
 */

/**	Returns the own 64-bit Serial Address of the XBee connected	**/
XBeeAddress64 LithneClass::getMyAddress64( bool forceCheck )
{
	/*	If the address is unknown, or if we force a recheck.	*/
	if( myAddress64.getLsb()	==	UNKNOWN_64B || forceCheck )
	{
		uint32_t msb	=	sendATCommand( atSH, 1000 );	//We want to wait for an answer
		uint32_t lsb	=	sendATCommand( atSL, 1000 );	//We want to wait for an answer
		myAddress64		=	XBeeAddress64( msb, lsb );
	}

	return myAddress64;
}
/*
  ___  ___    _ ___ ___ _____ ___ 
 / _ \| _ )_ | | __/ __|_   _/ __|
| (_) | _ \ || | _| (__  | | \__ \
 \___/|___/\__/|___\___| |_| |___/

 */

/**	Returns the node at the specific location in
	the node array.
	Argument: the location in the node array
 **/
Node * LithneClass::getNode( uint8_t position )
{
	if( position < MAX_NODES && position < numNodes )
	{
		return nodes[position];
	}
	else
	{
		return NULL;
	}
}


/**	Searches the node array for the node with specified 64-bit address. 
	If this node ID exists we return a pointer to that node, 
	otherwise we return a new node with an unkown node id	**/
Node * LithneClass::getNodeBy64( XBeeAddress64 _addr64 )
{
	for( int i=0; i<numNodes; i++ )
	{
		Node * curNode	=	getNode( i );
		if( curNode->getAddress64().getMsb() == _addr64.getMsb() &&
				curNode->getAddress64().getLsb() == _addr64.getLsb() )
		{
			return curNode;
		}
	}
	return NULL;
}

/**	Searches the node array for the node with specified 16-bit address. 
	If this node ID exists we return a pointer to that node, 
	otherwise we return a new node with an unkown node id	**/
Node * LithneClass::getNodeBy16( uint16_t _addr16 )
{
	for( int i=0; i<numNodes; i++ )
	{
		Node * curNode	=	getNode( i );
		if( curNode->getAddress16() == _addr16 )
		{
			return curNode;
		}
	}
	return NULL;
}

/**	Searches the node array for the node specified node ID. 
	If this node ID exists we return a pointer to that node, 
	otherwise we return a new node with an unkown node id	**/
Node * LithneClass::getNodeByID( uint8_t _id )
{
	for( int i=0; i<numNodes; i++ )
	{
		Node * curNode	=	getNode( i );
		if( curNode->getID() == _id )
		{
			return curNode;
		}
	}
	return NULL;
}

/*
 ___ ___ _____   ___ _____ ___   ___ _   _ _  _  ___ _____ ___ ___  _  _ ___ 
| _ \ _ \_ _\ \ / /_\_   _| __| | __| | | | \| |/ __|_   _|_ _/ _ \| \| / __|
|  _/   /| | \ V / _ \| | | _|  | _|| |_| | .` | (__  | |  | | (_) | .` \__ \
|_| |_|_\___| \_/_/ \_\_| |___| |_|  \___/|_|\_|\___| |_| |___\___/|_|\_|___/

 */

/** Look up the position of the node in the array, based on the ID **/
uint16_t LithneClass::getNodeArrayPosition( uint8_t _nodeId )
{
	for( uint8_t i=0; i<numNodes; i++)
	{
		if( getNode(i)->getID() == _nodeId )
		{
			return i;
		}
	}
	return UNKNOWN_NODE_ID;
}

/** Look up the position of the node in the array, based on the 64-bit address **/
uint16_t LithneClass::getNodeArrayPosition( uint16_t _addr16 )
{
	for( uint8_t i=0; i<numNodes; i++)
	{
		if( getNodeBy16( i )->getAddress16() == _addr16 )
		{
			return i;
		}
	}
	return UNKNOWN_NODE_ID;
}

/** Look up the position of the node in the array, based on the 64-bit address **/
uint16_t LithneClass::getNodeArrayPosition( XBeeAddress64 _addr64 )
{
	for( uint8_t i=0; i<numNodes; i++)
	{
		if( getNode( i )->getAddress64().getMsb() == _addr64.getMsb() &&
				getNode( i )->getAddress64().getLsb() == _addr64.getLsb() )
		{
			return i;
		}
	}
	return UNKNOWN_NODE_ID;
}


uint8_t lithne_db_s1;
/** This function takes care of all processing when a message is received.
	It is called after Lithne.available();
		This section is based on examples in the XBee library by Andrew Rapp**/
uint8_t LithneClass::getDBs1()
{
	return lithne_db_s1;
}

void LithneClass::readXbee()
{
	/*DONT EDIT IF YOU DON'T KNOW WHAT YOU'RE DOING!!!	*/

	/* Reads all available serial bytes until a packet is parsed, an error occurs, or the buffer is empty. */
	xbee.readPacket();

	//Serial.println(" - Reading XBEE packet" );

	if (xbee.getResponse().isAvailable()) //Returns a reference to the current response Note: once readPacket is called again this response will be overwritten!
	{
		XBeeResponse response = xbee.getResponse();
		uint8_t API_ID = xbee.getResponse().getApiId();
		//RxDataResponse* rx;


		if (API_ID == ZB_RX_RESPONSE || API_ID == RX_16_RESPONSE || API_ID == RX_64_RESPONSE) //we've got mail :)
		{

			/* Indicate we have received a new message */
			newMessage = true; // set flag

			/* Clear the old received message */
			incomingMessage->clearArguments();

			XBeeAddress64	addr64 = XBeeAddress64( 0, UNKNOWN_64B );
			uint16_t		addr16 = UNKNOWN_16B;
			RxDataResponse*		rx;


			//Get the responses, depending on the type of API
			if (API_ID == ZB_RX_RESPONSE)
			{
				ZBRxResponse zbrx = ZBRxResponse();

				//Call with instance of ZBRxResponse class only if getApiId() == ZB_RX_RESPONSE to populate response.
				response.getZBRxResponse(zbrx);
				rx = &zbrx;	//save to RxDataResponse, we need it later for retrieving the data

				/* Retrieve the sender from the packet*/
				addr64	=	zbrx.getRemoteAddress64();
				addr16	=	zbrx.getRemoteAddress16();
			}
			else if (API_ID == RX_16_RESPONSE)
			{
//				DEBUG_PRINTPRETTYLN("API_id = Rx16Response");
				Rx16Response rx16 = Rx16Response();

				response.getRx16Response(rx16);
				rx =&rx16; //save to RxDataResponse, we need it later for retrieving the data


				//save db of message
				lithne_db_s1 = rx16.getRssi();

				addr16 = rx16.getRemoteAddress16();
//				DEBUG_PRINTPRETTYLN("address retreived.");
			}
			else if (API_ID == RX_64_RESPONSE)
			{
//				DEBUG_PRINTPRETTYLN("API_id = Rx64Response");
				Rx64Response rx64 = Rx64Response();
				response.getRx64Response(rx64);
				rx=&rx64; //save to RxDataResponse, we need it later for retrieving the data

				//save db of message
				lithne_db_s1 = rx64.getRssi();

				addr64 = rx64.getRemoteAddress64();
			}

			//			Store the addres of the sender of the message
			incomingMessage->setSender( addr16, addr64 );

			/*	The scope of the message is stored in the first two bytes
			of the payload.	Here we retrieve this and write it to the
			incoming message	*/
			incomingMessage->setScope( (rx->getData(0) << 8) + rx->getData(1) );

			/* 	The function identifier (1 byte) is stored in the third
									byte of the payload. Here we retrieve this and write it to
									the incoming message	*/
			incomingMessage->setFunction(rx->getData(2));

			/*	The remainder of the payload contains our arguments. Here
						      		we retrieve the number of arguments, by subtracting the
						      		first three bytes (which contain the scope and the function
						      		ID) and dividing the number of bytes by two, since we send
						      		16-bit values and not 8-bit values (2x 8-bits = 1x 16bits)	*/
			uint16_t numOfBytes	=	(rx->getDataLength()-3);
			uint16_t numOfArgs	=	numOfBytes/2;

			/* Store the arguments in the incomingMessage */
			for( int i = 0; i < numOfArgs; i++ )
			{
				uint16_t pos    =	(i*2) + 3;
				uint16_t data 	=	(rx->getData(pos) << 8) + rx->getData(pos+1);

//				Serial.print("Arguments ");
//				Serial.print(i);
//				Serial.print(" = ");
//				Serial.println(data, HEX);

				incomingMessage->addArgument(data);
			}

			/*	Here we always overwrite the 16-bit address. The received address
						    		is directly taken from the header information and thus correct.	*/
			getNodeBy64( addr64 )->setAddress16( addr16 );



			//todo: fix this getOption is not a member of RxDataResponse, not urgent error is not handled now anyway
			//			/* If the sender(!) of the message got an acknowledgement, this code is executed */
			//			if (rx->getOption() == ZB_PACKET_ACKNOWLEDGED)
			//			{
			//
			//			}
			//			/*	If not, something strange has happened, because we got the message, but
			//									the sender did not receive an acknowledgement */
			//			else
			//			{
			//				//we got it (obviously) but sender didn't get an ACK
			//			}


		}


		/*	If the packet is indeed an AT_COMMAND_RESPONSE,	we write it to the atResponse.	*/
		else if (API_ID == AT_COMMAND_RESPONSE)
		{
			//		Serial.println("Received AT answer");
			AtCommandResponse atResponse 	= AtCommandResponse();
			uint32_t atAnswer;

			response.getAtCommandResponse( atResponse );

			/*	If the atResponse is correct (does not return an error code), we can process it.	*/
			if (atResponse.isOk())
			{
				if (atResponse.getValueLength() > 0)
				{
					for (int i = 0; i < atResponse.getValueLength(); i++)
					{
						atAnswer	=	(atAnswer << 8) + atResponse.getValue()[i];
					}
				}

				/*	If we got a Serial High	('S','H')	*/
				if( atResponse.getCommand()[0] == atSH[0] &&
						atResponse.getCommand()[1] == atSH[1] )
				{
					myAddress64.setMsb( atAnswer );
				}
				/*	If we got a Serial LOW ('S','L')	*/
				if( atResponse.getCommand()[0] == atSL[0] &&
						atResponse.getCommand()[1] == atSL[1] )
				{
					myAddress64.setLsb( atAnswer );
				}
				/*	If we got my 16-bit address ('M','Y')	*/
				if( atResponse.getCommand()[0] == atMY[0] &&
						atResponse.getCommand()[1] == atMY[1] )
				{
					myAddress16	=	atAnswer & 0xFFFF;
				}
				/*	If we got my 16-bit address ('M','Y')	*/
				if( atResponse.getCommand()[0] == atID[0] &&
						atResponse.getCommand()[1] == atID[1] )
				{
					myPANid		=	atAnswer & 0xFFFF;
				}
				/*	If we got my 16-bit address ('M','Y')	*/
				if( atResponse.getCommand()[0] == atAI[0] &&
						atResponse.getCommand()[1] == atAI[1] )
				{
					myAssStat	=	atAnswer & 0xFFFF;
				}
			}
			else {	}
		}

		/* NODE JOINING/LEAVING THE NETWORK */
		else if( API_ID == MODEM_STATUS_RESPONSE )
		{
			response.getModemStatusResponse(msr);

			if (msr.getStatus() == ASSOCIATED)
			{
				/* A new node has joined the network.
		        		Here we add the node if it is not yet known and store the
		        		64-bit and 16-bit address
				 */
				//        	xbee.getResponse().;
			}
			else if (msr.getStatus() == DISASSOCIATED)
			{
				/* Node leaves the network
				 */
			}
		}
		/* DB MEASUREMENT RESPONSE */
		else if( API_ID == REMOTE_AT_COMMAND_RESPONSE ) 	//REMOTE_AT_COMMAND_RESPONSE
		{
			response.getRemoteAtCommandResponse(rATcmd);

			if ( rATcmd.getCommand()[0]	==	atDB[0] &&
					rATcmd.getCommand()[1]	==	atDB[1] )
			{
				XBeeAddress64 rAddress64	=	rATcmd.getRemoteAddress64();
				uint16_t rAddress16			=	rATcmd.getRemoteAddress16();

				getNodeBy64( rAddress64 )->addDBMeasurement( rATcmd.getValue()[0] );

				/*	The line above replaces this section
					for (int i = 0; i < numNodes; i++)
					{
						if( nodes[i]->getID() == remoteId )
						{
			    			nodes[i]->addDBMeasurement( rATcmd.getValue()[0] );
				    	}
			      	}
				 */

				if( !nodeKnown16( rAddress16 ) && nodeKnown64( rAddress64 ) )
				{
					getNodeBy64( rAddress64 )->setAddress16( rAddress16 );
					//Serial.print("We know the 64-bit address, but not the 16-bit");
				}
			}
			/*	Here we can add other remote AT command responses
			    else if( rATcmd.getCommand()[0]	==	atSH[0] &&
		      		 	 rATcmd.getCommand()[1]	==	atSH[1] )
				{

				}
			 */
		}
		/*	Confirmation on transmitted package. This is received
		    	everytime a message is transmitted and has details
		    	whether the package is received or not.
		 */
		//todo: edit for TX_STATUS_RESPONSE
		else if (API_ID == ZB_TX_STATUS_RESPONSE)
		{
			ZBTxStatusResponse txStatus = ZBTxStatusResponse();
			response.getZBTxStatusResponse(txStatus);

			if (txStatus.getDeliveryStatus() == SUCCESS)
			{
				uint16_t rAddress16	=	txStatus.getRemoteAddress();	//Stores the 16-bit address

				if( !nodeKnown16( rAddress16 ) )
				{
					//Here we send something that requests data from the remote node
					sendDBRequest16( rAddress16 );
					/*Serial.print("Received TX-Stat-Repsonse from unknown 16B add: ");
			 			Serial.print( rAddress16 );
			 			Serial.println(" -> Sending DB Req");*/
				}
				/*
			 		Als we hier zijn aangekomen, hebben we een berichtje gestuurd,
			 		dat is goed aangekomen. Deze response geeft echter alleen een
			 		16-bit address terug en geen 64-bit, dus die kunnen we niet aan
			 		een node koppelen.
			 		Nu willen we kijken of de 16-bit bekend is in onze node lijst.
			 		Als dat zo is, dan hoeven we niets te doen, we kennen deze node.
			 		Als dat NIET zo is, dan willen we van deze node ook het 64-bit
			 		address opvragen, zodat we 16-bit en 64-bit op kunnen slaan.

					if( last16B	==	UNKNOWN_16B )
					{
						setNodeAddress( last64B , txStatus.getRemoteAddress() );
					}
				 */
			}
			else
			{
				/* the remote XBee did not receive our packet.
		        		If this is because the 16 bit address is outdated; we wish to reset that

					setNodeAddress( last64B , UNKNOWN_16B );
				 */
			}
			/* Retrieve the sender from the packet and store it in the message *
		     	XBeeAddress64	add64	=	outgoingMessage->getRecipient();
		     	uint16_t		add16	=	txStatus.getRemoteAddress();

		    	/* Here we write the 16-bit address if the node does not yet have this *
		    	setNodeAddress( add64, add16 );

		    	/*
		    	Dit kan fout gaan als de outgoingMessage al is overschreven, tussen
		    	het versturen van de outgoingMessage en het ontvangen van de
		    	incomingMessage. We kunnen echter uit de txStatus niet de 64-bit address
		    	opvragen.
			 */
		}
		//Something occured that is unknown, or we do not care about
		else
		{
		}
	}
}



/* Define an instance of the Lithneclass here, named Lithne */
LithneClass Lithne(S1);
